from ament_index_python.packages import get_package_share_directory
from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument
# execute
from launch.actions import ExecuteProcess
from launch.substitutions import LaunchConfiguration
from launch_ros.actions import Node


def generate_launch_description():
    realsense_node= LaunchDescription([
        Node(
                package='realsense2_camera',  # 你使用的相机驱动程序包的名称   ros2 run realsense2_camera realsense2_camera_node    #realsense d435

                executable='realsense2_camera_node',         # 相机节点的可执行文件名
                name='camera',
                output='screen',
                parameters=[{
                    'depth_module.emitter_enabled': 'Off'  # 将参数设置为 Off
                }],
                remappings=[
                    ('/camera/camera/infra1/camera_info', '/stereo/left/camera_info'),
                    ('/camera/camera/infra1/image_rect_raw', '/stereo/left/image_raw'),
                    ('/camera/camera/infra2/camera_info', '/stereo/right/camera_info'),
                    ('/camera/camera/infra2/image_rect_raw', '/stereo/right/image_raw'),
                ]
            )
        ])
    camera_calibration_node = ExecuteProcess(
        cmd=[
            'ros2', 'run', 'camera_calibration', 'cameracalibrator',
            '--approximate', '0.1',
            '--size', '11x8',
            '--square', '0.02',
            '--ros-args', '--remap', 'right:=/stereo/right/image_raw',
            '--ros-args', '--remap', 'left:=/stereo/left/image_raw',
            '--ros-args', '--remap', 'right_camera:=/stereo/right',
            '--ros-args', '--remap', 'left_camera:=/stereo/left'
        ],
        output='screen'
    )
    return LaunchDescription([realsense_node,camera_calibration_node])

